#include <algorithm>
#include "gc.h"
//------------------------------------------------------------------------------
Point::Point (int x, int y)
{
  set(x, y);
}



Point::Point (LabelImage::IndexType index)
{
  set(index[0], index[1]);
}



void Point::set (int x, int y)
{
  this->x = x;
  this->y = y;
}



bool Point::isEqual (Point point)
{
  if (x == point.x && y == point.y) {
    return true;
  }
  return false;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Component::Component (int no, bool isOnBoundary)
{
  set(no, isOnBoundary);
}



Component::Component (const Component& component)
{
  no = component.no;
  isOnBoundary = component.isOnBoundary;
  for (ComponentPointSet::const_iterator itr = component.points.begin(); 
       itr != component.points.end(); itr++) {
    points.push_back(*itr);
  }
}



void Component::set (int no, bool isOnBoundary)
{
  this->no = no;
  this->isOnBoundary = isOnBoundary;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Layer::Layer (int no, LabelImage::Pointer labelImage)
{
  set(no, labelImage);
}



Layer::Layer (const Layer& layer)
{
  no = layer.no;
  components.reserve(layer.components.size());
  for (LayerComponentSet::const_iterator itr = layer.components.begin(); 
       itr != layer.components.end(); itr++) {
    components.push_back(*itr);
  }
  labelImage = layer.labelImage;
}



void Layer::set (int no, LabelImage::Pointer labelImage)
{
  this->no = no;
  this->labelImage = labelImage;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Volume::Volume (int startSliceNo, int startLayerNo, int layerNum)
{
  this->startSliceNo = startSliceNo;
  this->startLayerNo = startLayerNo;
  this->layerNum = layerNum;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Edge::Edge (int endNodeNo, int area, double weight_area, 
	    double correlation, double weight_correlation, double weight)
{
  set(endNodeNo, area, weight_area, correlation, weight_correlation, weight);
}



void Edge::set (int endNodeNo, int area, double weight_area, 
		double correlation, double weight_correlation, double weight)
{
  this->endNodeNo = endNodeNo;
  this->area = area;
  this->weight_area = weight_area;
  this->correlation = correlation;
  this->weight_correlation = weight_correlation;
  this->weight = weight;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Node::Node (int no, int layerNo, int componentNo, NodeStatus status)
{
  set(no, layerNo, componentNo, status);
}



Node::Node (const Node& node)
{
  set(node.no, node.layerNo, node.componentNo, node.status);
  for (NodeEdgeSet::const_iterator itr = node.edges.begin(); 
       itr != node.edges.end(); itr++) {
    edges.push_back(*itr);
  }
}



void Node::set (int no, int layerNo, int componentNo, NodeStatus status)
{
  this->no = no;
  this->layerNo = layerNo;
  this->componentNo = componentNo;
  this->status = status;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
RawNode::RawNode (int layerNo, int nodeNo)
{
  set(layerNo, nodeNo);
}



void RawNode::set (int layerNo, int nodeNo)
{
  this->layerNo = layerNo;
  this->nodeNo = nodeNo;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Graph3D::Graph3D ()
{
  body = NULL;
  bodyStartLayerNo = -1;
  bodyLayerNum = -1;
  bodyNodeNum = -1;
  bodyEdgeNum = -1;
}



Graph3D::~Graph3D ()
{
  destroy();
}



void Graph3D::changeNodeStatus (SegmentationGraph const& segGraph, NodeStatus status)
{
  for (SegGraphSegNodeSet::const_iterator itr = segGraph.segNodeSet.begin(); 
       itr != segGraph.segNodeSet.end(); itr++) {
    nodes[itr->layerNo][itr->nodeNo].status = status;
  }
}



void Graph3D::addNode (Node const& node)
{
  nodes[node.layerNo].push_back(node);
}



void Graph3D::addEdge (int layerNo, int nodeNo, Edge const& edge)
{
  nodes[layerNo][nodeNo].edges.push_back(edge);
  Edge reverseEdge(nodeNo, edge.area, edge.weight_area, 
		   edge.correlation, edge.weight_correlation, edge.weight);
  nodes[layerNo + 1][edge.endNodeNo].reverseEdges.push_back(reverseEdge);
}



void Graph3D::addSkipEdge (int layerNo, int nodeNo, Edge const& skipEdge)
{
  nodes[layerNo][nodeNo].skipEdges.push_back(skipEdge);
  Edge reverseSkipEdge(nodeNo, skipEdge.area, skipEdge.weight_area, 
		       skipEdge.correlation, skipEdge.weight_correlation, 
		       skipEdge.weight);
  nodes[layerNo + 2][skipEdge.endNodeNo].reverseSkipEdges.push_back(reverseSkipEdge);
}



void Graph3D::create (int bodyStartLayerNo, int bodyLayerNum, 
		      GraphType graphType)
{
  destroy();
  this->bodyStartLayerNo = bodyStartLayerNo;
  this->bodyLayerNum = bodyLayerNum;
  bodyNodeNum = 0;
  bodyEdgeNum = 0;
  // Create foward graph
  if (graphType == GRAPH_FWD) {
    int bodyEndLayerNo = bodyStartLayerNo + bodyLayerNum - 1;
    std::map<int, int> rnc;
    rawNodeCorrespondence.assign(bodyLayerNum, rnc);
    for (int layerNo = bodyStartLayerNo; layerNo <= bodyEndLayerNo; layerNo++) {
      for (GraphLayerNodeSet::const_iterator nitr = nodes[layerNo].begin(); 
	   nitr != nodes[layerNo].end(); nitr++) {
	if (nitr->status == NODE_NORMAL) {
	  bodyNodeNum++;
	  if (layerNo < bodyEndLayerNo) {
	    for (NodeEdgeSet::const_iterator eitr = nitr->edges.begin(); 
		 eitr != nitr->edges.end(); eitr++) {
	      if (nodes[layerNo + 1][eitr->endNodeNo].status == NODE_NORMAL) {
		bodyEdgeNum++;
	      }
	    }
	    if (layerNo < bodyEndLayerNo - 1) {
	      for (NodeEdgeSet::const_iterator eitr = nitr->skipEdges.begin(); 
		   eitr != nitr->skipEdges.end(); eitr++) {
		if (nodes[layerNo + 2][eitr->endNodeNo].status == NODE_NORMAL) {
		  bodyEdgeNum++;
		}
	      }
	    }
	  }
	}
      }
    }
    nodeCorrespondence.reserve(bodyNodeNum);
    body = new RawGraph(bodyNodeNum, bodyEdgeNum);
    // Add nodes
    for (int layerNo = bodyStartLayerNo; layerNo <= bodyEndLayerNo; layerNo++) {
      for (int nodeNo = 0; nodeNo < nodes[layerNo].size(); nodeNo++) {
	if (nodes[layerNo][nodeNo].status == NODE_NORMAL 
	    && nodes[layerNo][nodeNo].componentNo > 0) {
	  int rawNodeNo = body->add_node();
	  RawNode rawNode(layerNo, nodeNo);
	  nodeCorrespondence.push_back(rawNode);
	  rawNodeCorrespondence[layerNo - bodyStartLayerNo][nodeNo] = rawNodeNo;
	}
      }
    }
    // Add edges
    for (int layerNo = bodyStartLayerNo; layerNo < bodyEndLayerNo; layerNo++) {
      for (int nodeNo = 0; nodeNo < nodes[layerNo].size(); nodeNo++) {
	if (nodes[layerNo][nodeNo].status == NODE_NORMAL 
	    && nodes[layerNo][nodeNo].componentNo > 0) {
	  for (NodeEdgeSet::iterator itr = nodes[layerNo][nodeNo].edges.begin(); 
	       itr != nodes[layerNo][nodeNo].edges.end(); itr++) {
	    if (nodes[layerNo + 1][itr->endNodeNo].status == NODE_NORMAL 
		&& nodes[layerNo + 1][itr->endNodeNo].componentNo > 0) {
	      body->add_edge(rawNodeCorrespondence[layerNo - bodyStartLayerNo][nodeNo], 
			     rawNodeCorrespondence[layerNo - bodyStartLayerNo + 1][itr->endNodeNo], 
			     itr->weight, itr->weight);
	    }
	  }
	  // Add skip edges
	  if (layerNo < bodyEndLayerNo - 1) {
	    for (NodeEdgeSet::iterator itr = nodes[layerNo][nodeNo].skipEdges.begin(); 
		 itr != nodes[layerNo][nodeNo].skipEdges.end(); itr++) {
	      if (nodes[layerNo + 2][itr->endNodeNo].status == NODE_NORMAL 
		  && nodes[layerNo + 2][itr->endNodeNo].componentNo > 0) {
		body->add_edge(rawNodeCorrespondence[layerNo - bodyStartLayerNo][nodeNo], 
			       rawNodeCorrespondence[layerNo - bodyStartLayerNo + 2][itr->endNodeNo], 
			       itr->weight, itr->weight);
	      }
	    }
	  }
	}
      }
    }
  }
  // Create backward graph
  else if (graphType == GRAPH_BWD) {
    int bodyEndLayerNo = bodyStartLayerNo - bodyLayerNum + 1;
    std::map<int, int> rnc;
    rawNodeCorrespondence.assign(bodyLayerNum, rnc);
    for (int layerNo = bodyStartLayerNo; layerNo >= bodyEndLayerNo; layerNo--) {
      for (GraphLayerNodeSet::const_iterator nitr = nodes[layerNo].begin(); 
	   nitr != nodes[layerNo].end(); nitr++) {
	if (nitr->status == NODE_NORMAL) {
	  bodyNodeNum++;
	  if (layerNo > bodyEndLayerNo) {
	    for (NodeEdgeSet::const_iterator eitr = nitr->reverseEdges.begin(); 
		 eitr != nitr->reverseEdges.end(); eitr++) {
	      if (nodes[layerNo - 1][eitr->endNodeNo].status == NODE_NORMAL) {
		bodyEdgeNum++;
	      }
	    }
	    if (layerNo > bodyEndLayerNo + 1) {
	      for (NodeEdgeSet::const_iterator eitr = nitr->reverseSkipEdges.begin(); 
		   eitr != nitr->reverseSkipEdges.end(); eitr++) {
		if (nodes[layerNo - 2][eitr->endNodeNo].status == NODE_NORMAL) {
		  bodyEdgeNum++;
		}
	      }
	    }
	  }
	}
      }
    }
    nodeCorrespondence.reserve(bodyNodeNum);
    body = new RawGraph(bodyNodeNum, bodyEdgeNum);
    // Add nodes
    for (int layerNo = bodyStartLayerNo; layerNo >= bodyEndLayerNo; layerNo--) {
      for (int nodeNo = 0; nodeNo < nodes[layerNo].size(); nodeNo++) {
	if (nodes[layerNo][nodeNo].status == NODE_NORMAL 
	    && nodes[layerNo][nodeNo].componentNo > 0) {
	  int rawNodeNo = body->add_node();
	  RawNode rawNode(layerNo, nodeNo);
	  nodeCorrespondence.push_back(rawNode);
	  rawNodeCorrespondence[bodyStartLayerNo - layerNo][nodeNo] = rawNodeNo;
	}
      }
    }
    // Add edges
    for (int layerNo = bodyStartLayerNo; layerNo > bodyEndLayerNo; layerNo--) {
      for (int nodeNo = 0; nodeNo < nodes[layerNo].size(); nodeNo++) {
	if (nodes[layerNo][nodeNo].componentNo > 0) {
	  for (NodeEdgeSet::iterator itr = nodes[layerNo][nodeNo].reverseEdges.begin(); 
	       itr != nodes[layerNo][nodeNo].reverseEdges.end(); itr++) {
	    if (nodes[layerNo - 1][itr->endNodeNo].status == NODE_NORMAL 
		&& nodes[layerNo - 1][itr->endNodeNo].componentNo > 0) {
	      body->add_edge(rawNodeCorrespondence[bodyStartLayerNo - layerNo][nodeNo], 
			     rawNodeCorrespondence[bodyStartLayerNo - layerNo + 1][itr->endNodeNo], 
			     itr->weight, itr->weight);
	    }
	  }
	  // Add skip edges
	  if (layerNo > bodyEndLayerNo + 1) {
	    for (NodeEdgeSet::iterator itr = nodes[layerNo][nodeNo].reverseSkipEdges.begin(); 
		 itr != nodes[layerNo][nodeNo].reverseSkipEdges.end(); itr++) {
	      if (nodes[layerNo - 2][itr->endNodeNo].status == NODE_NORMAL 
		  && nodes[layerNo - 2][itr->endNodeNo].componentNo > 0) {
		body->add_edge(rawNodeCorrespondence[bodyStartLayerNo - layerNo][nodeNo], 
			       rawNodeCorrespondence[bodyStartLayerNo - layerNo + 2][itr->endNodeNo], 
			       itr->weight, itr->weight);
	      }
	    }
	  }
	}
      }
    }
  }
  else {
    std::cerr << "Error: invalid graph type." << std::endl;
  }
}



void Graph3D::create (int startLayerNo, int layerNum)
{
  destroy();
  bodyStartLayerNo = startLayerNo;
  bodyLayerNum = layerNum;
  bodyNodeNum = 0;
  bodyEdgeNum = 0;
  int bodyEndLayerNo = bodyStartLayerNo + bodyLayerNum - 1;
  std::map<int, int> rnc;
  rawNodeCorrespondence.assign(bodyLayerNum, rnc);
  for (GraphNodeSet::const_iterator litr = nodes.begin(); 
       litr != nodes.end(); litr++) {
    for (GraphLayerNodeSet::const_iterator nitr = litr->begin(); 
	 nitr != litr->end(); nitr++) {
      if (nitr->status == NODE_NORMAL && nitr->componentNo > 0) {
	bodyNodeNum++;
	if (nitr->layerNo < bodyEndLayerNo) {
	  for (NodeEdgeSet::const_iterator eitr = nitr->edges.begin(); 
	       eitr != nitr->edges.end(); eitr++) {
	    if (nodes[nitr->layerNo][eitr->endNodeNo].status == NODE_NORMAL 
		&& nodes[nitr->layerNo][eitr->endNodeNo].componentNo > 0) {
	      bodyEdgeNum++;
	    }
	  }
	  if (nitr->layerNo < bodyEndLayerNo - 1) {
	    for (NodeEdgeSet::const_iterator eitr = nitr->skipEdges.begin(); 
		 eitr != nitr->skipEdges.end(); eitr++) {
	      if (nodes[nitr->layerNo][eitr->endNodeNo].status == NODE_NORMAL 
		  && nodes[nitr->layerNo][eitr->endNodeNo].componentNo > 0) {
		bodyEdgeNum++;
	      }
	    }
	  }
	}
      }
    }
  }
  nodeCorrespondence.reserve(bodyNodeNum);
  body = new RawGraph(bodyNodeNum, bodyEdgeNum);
  // Add nodes
  for (GraphNodeSet::const_iterator litr = nodes.begin(); 
       litr != nodes.end(); litr++) {
    for (GraphLayerNodeSet::const_iterator nitr = litr->begin(); 
	 nitr != litr->end(); nitr++) {
      if (nitr->status == NODE_NORMAL && nitr->componentNo > 0) {
	rawNodeCorrespondence[nitr->layerNo - bodyStartLayerNo][nitr->no] = 
	  body->add_node();
	RawNode rawNode(nitr->layerNo, nitr->no);
	nodeCorrespondence.push_back(rawNode);
      }
    }
  }
  // Add edges
  for (GraphNodeSet::const_iterator litr = nodes.begin(); 
       litr != nodes.end(); litr++) {
    for (GraphLayerNodeSet::const_iterator nitr = litr->begin(); 
	 nitr != litr->end(); nitr++) {
      if (nitr->layerNo < bodyEndLayerNo) {
	for (NodeEdgeSet::const_iterator eitr = nitr->edges.begin(); 
	     eitr != nitr->edges.end(); eitr++) {
	  if(nodes[nitr->layerNo + 1][eitr->endNodeNo].status == NODE_NORMAL 
	     && nodes[nitr->layerNo + 1][eitr->endNodeNo].componentNo > 0) {
	    body->add_edge(rawNodeCorrespondence[nitr->layerNo - bodyStartLayerNo][nitr->no], rawNodeCorrespondence[nitr->layerNo + 1 - bodyStartLayerNo][eitr->endNodeNo], eitr->weight, eitr->weight);
	  }
	}
	// Add skip edges
	if (nitr->layerNo < bodyEndLayerNo - 1) {
	  for (NodeEdgeSet::const_iterator eitr = nitr->skipEdges.begin(); 
	       eitr != nitr->skipEdges.end(); eitr++) {
	    if (nodes[nitr->layerNo + 2][eitr->endNodeNo].status == NODE_NORMAL 
		&& nodes[nitr->layerNo + 2][eitr->endNodeNo].componentNo > 0) {
	      body->add_edge(rawNodeCorrespondence[nitr->layerNo - bodyStartLayerNo][nitr->no], rawNodeCorrespondence[nitr->layerNo + 2 - bodyStartLayerNo][eitr->endNodeNo], eitr->weight, eitr->weight);
	    }
	  }
	}
      }
    }
  }
}



void Graph3D::destroy ()
{
  if (body != NULL) {
    delete body;
    body = NULL;
  }
  nodeCorrespondence.clear();
  for (RawNodeCorrespondence::iterator itr = rawNodeCorrespondence.begin(); 
       itr != rawNodeCorrespondence.end(); itr++) {
    itr->clear();
  }
  rawNodeCorrespondence.clear();
  graphType = GRAPH_NONE;
  bodyStartLayerNo = -1;
  bodyLayerNum = -1;
  bodyNodeNum = -1;
  bodyEdgeNum = -1;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
SegmentationNode::SegmentationNode (int layerNo, int nodeNo, 
				    SegmentationType segType)
{
  set(layerNo, nodeNo, segType);
}



void SegmentationNode::set (int layerNo, int nodeNo, SegmentationType segType)
{
  this->layerNo = layerNo;
  this->nodeNo = nodeNo;
  this->segType = segType;
}


bool SegmentationNode::isEqual (SegmentationNode const& segNode)
{
  if (layerNo == segNode.layerNo && nodeNo == segNode.nodeNo) {
    return true;
  }
  return false;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
SegmentationGraph::SegmentationGraph (const SegmentationGraph& segGraph)
{
  segNodeSet.clear();
  for (SegGraphSegNodeSet::const_iterator itr = segGraph.segNodeSet.begin(); 
       itr != segGraph.segNodeSet.end(); itr++) {
    segNodeSet.push_back(*itr);
  }
}



bool compareSegNode (SegmentationNode const& segNode0, 
		     SegmentationNode const& segNode1)
{
  if (segNode0.layerNo < segNode1.layerNo) {
    return true;
  }
  else if (segNode0.layerNo == segNode1.layerNo) {
    return (segNode0.nodeNo < segNode1.nodeNo);
  }
  return false;
}



void SegmentationGraph::addSegNode (SegmentationNode const& segNode, bool isScan)
{
  if (isScan) {
    for (SegGraphSegNodeSet::iterator itr = segNodeSet.begin(); 
	 itr != segNodeSet.end(); itr++) {
      if (itr->isEqual(segNode)) {
	if (itr->segType != segNode.segType && segNode.segType != SEG_NONE) {
	  if (itr->segType == SEG_NONE) {
	    itr->segType = segNode.segType;
	  }
	  else {
	    itr->segType = SEG_BOTH;
	  }
	}
	return;
      }
    }
  }
  segNodeSet.push_back(segNode);
}



void SegmentationGraph::sort ()
{
  segNodeSet.sort(compareSegNode);
}



SegmentationGraph SegmentationGraph::getIntersection (SegmentationGraph& segGraph)
{
  this->sort();
  segGraph.sort();
  SegmentationGraph ret;
  ret.segNodeSet.resize(segNodeSet.size() + segGraph.segNodeSet.size());
  SegGraphSegNodeSet::iterator _end = 
    set_intersection(segNodeSet.begin(), segNodeSet.end(), 
		     segGraph.segNodeSet.begin(), segGraph.segNodeSet.end(), 
		     ret.segNodeSet.begin(), compareSegNode);
  ret.segNodeSet.erase(_end, ret.segNodeSet.end());
  return ret;
}



SegmentationGraph SegmentationGraph::getUnion (SegmentationGraph& segGraph)
{
  segNodeSet.sort(compareSegNode);
  segGraph.segNodeSet.sort(compareSegNode);
  SegmentationGraph ret;
  ret.segNodeSet.resize(segNodeSet.size() + segGraph.segNodeSet.size());
  SegGraphSegNodeSet::iterator _end = 
    set_union(segNodeSet.begin(), segNodeSet.end(), 
	      segGraph.segNodeSet.begin(), segGraph.segNodeSet.end(), 
	      ret.segNodeSet.begin(), compareSegNode);
  ret.segNodeSet.erase(_end, ret.segNodeSet.end());
  return ret;
}



SegmentationGraph SegmentationGraph::getDifference (SegmentationGraph& segGraph)
{
  segNodeSet.sort(compareSegNode);
  segGraph.segNodeSet.sort(compareSegNode);
  SegmentationGraph ret;
  ret.segNodeSet.resize(segNodeSet.size() + segGraph.segNodeSet.size());
  SegGraphSegNodeSet::iterator _end = 
    set_difference(segNodeSet.begin(), segNodeSet.end(), 
		   segGraph.segNodeSet.begin(), segGraph.segNodeSet.end(), 
		   ret.segNodeSet.begin(), compareSegNode);
  ret.segNodeSet.erase(_end, ret.segNodeSet.end());
  return ret;
}



SegmentationGraph SegmentationGraph::getCertainTypeSegGraph (SegmentationType type)
{
  SegmentationGraph ret;
  for (SegGraphSegNodeSet::const_iterator itr = segNodeSet.begin(); 
       itr != segNodeSet.end(); itr++) {
    if (itr->segType == type 
	|| ((type == SEG_FWD || type == SEG_BWD) && itr->segType == SEG_BOTH)) {
      ret.segNodeSet.push_back(*itr);
    }
  }
  return ret;
}
